#include "custommeter.hpp"
#include "method.hpp"
#include "threados.hpp"
#include <math.h>


using namespace systemos;
using namespace measure;

extern "C"{
    #include "SEGGER_RTT.h"
    #include "app_config.h"
}

#define DBG_CURRENT_TEST

bool custommeter::mLock = false;
bool custommeter::mFirstComplated = false;
uint32_t custommeter::mIndex = 0;
adc* custommeter::mAdc = 0;
uint32_t custommeter::ChannelCount = 0;
uint32_t custommeter::Frequnce = 0;
uint32_t custommeter::SampleLen = 0;
uint32_t custommeter::EfectiveSampleLen = 0;
uint16_t* custommeter::SampleBuff = 0;
uint16_t* custommeter::EfectiveSampleBuff = 0;
custommeter::MeterChannelTPDF custommeter::MeterChannel[CUSTOMMETER_CHANNEL_MAX_SURPPORT];
custommeter::ValueOutOfLimitDelegate custommeter::ValueOutOfLimitCallback = 0;
custommeter::ValueConvertedDelegate custommeter::ValueConvertedCallback = 0;

short custommeter::calculate::SmpPeak[CUSTOMMETER_CHANNEL_MAX_SURPPORT];
short custommeter::calculate::SmpRms[CUSTOMMETER_CHANNEL_MAX_SURPPORT];

arm_matrix_instance_q15 custommeter::calculate::MatSrc;
arm_matrix_instance_q15 custommeter::calculate::MatDst;

short custommeter::calculate::SampleArrayBuf[ADC_CYC_DATA_NUM];

short custommeter::calculate::Ifrm;
short custommeter::calculate::In;
short custommeter::calculate::Ii;
short custommeter::calculate::Iin;
short custommeter::calculate::CurrPhPosBuf[CUSTOMMETER_CHANNEL_MAX_SURPPORT];
short custommeter::calculate::CurrPhPos[CUSTOMMETER_CHANNEL_MAX_SURPPORT];    
short custommeter::calculate::AdcDataPos;
const struct custommeter::calculate::LinearTable* custommeter::calculate::pLinearTable = NULL;

short custommeter::calculate::FilterArrayBuf[16+CUR_FILTR_NUM];
short custommeter::calculate::AdcPeakBuf[CUSTOMMETER_CHANNEL_MAX_SURPPORT];
short custommeter::calculate::AdcMidLvlBuf[CUSTOMMETER_CHANNEL_MAX_SURPPORT];
short custommeter::calculate::CurrentVal[CUSTOMMETER_CHANNEL_MAX_SURPPORT];
short custommeter::calculate::CurrentBuf[CURRENT_NUM];


short custommeter::calculate::IilLvl;
short custommeter::calculate::IinlLvl;
short custommeter::calculate::IihLvl;
short custommeter::calculate::IinhLvl;

char custommeter::calculate::Ch0Flag;
char custommeter::calculate::Ch1Flag;

bool custommeter::calculate::StartFilterFlag = false;

const struct custommeter::calculate::LinearIndex custommeter::calculate::LinearIndexArray[] = {
  // Ie         In          Index
//  /** 250-32*/
//  {250,       32,        0},
//  {250,       40,        0},
//  {250,       50,        0},
//  /** 250-63*/
//  {250,       63,        0},
//  /** 250-100*/
//  {250,       80,        0},
//  {250,       100,        0},
//  /** 250-160*/
//  {250,       125,        0},
//  {250,       160,        0},
//  /** 250-250*/
//  {250,       180,        0},
//  {250,       200,        0},
//  {250,       225,        0},
    {250,       250,        0},
//  /** 630-250*/
//  {630,       250,        1},
//  /** 630-400*/
//  {630,       315,        1},
//  {630,       350,        1},
    {630,       400,        1},
//  /** 630-630*/
    {630,       500,        2},
    {630,       630,        2},
//  /** 800-630*/
    {800,       630,        3},
    {800,       800,        3},
    /** 1600-800*/
    {1600,       800,       4},
    /** 1600-1000*/
    {1600,       1000,      4},
    /** 1600-1250*/
    {1600,       1250,      4},
    /** 1600-1600*/
    {1600,       1600,      4},
};


const struct custommeter::calculate::LinearTable custommeter::calculate::LinearTableArray[] = {
{{///<250-250
    {/*Ch 0 RMS*/
        {},
        0,
        /**/
        SMP_CMP,        CAL_PH_ABCN,        {0,  0 }
    },
    {/*Ch 0 PEAK*/
        {},
        0,     
        SMP_CMP,        CAL_PH_ABCN,        {0,  0 }
    },
    {/*Ch 1 RMS*/
        {{0,    0    }, {100,  68   }, {201,  138  }, {301,  203  }, {501,  336  }, {753,  505  }, {1008, 675  }, {1263, 834  }, {1510, 1002 }, {1758, 1153 }, 
         {2002, 1247 }, {2255, 1321 }, {2500, 1392 }, {2747, 1428 }, {3000, 1473 }, {3270, 1542 }, {3508, 1622 }, {3749, 1638 }, {4017, 1668}},
        19,
         /**/
        SMP_DC,         CAL_PH_ABCN,        {0,  3 }
    },
    {/*Ch 1 PEAK*/
        {{0,    0    }, {100,  98   }, {201,  192  }, {301,  285  }, {501,  472  }, {753,  709  }, {1008, 950  }, {1263, 1178 }, {1510, 1417 }, {1758, 1651 }, 
         {2002, 1875 }, {2255, 2116 }, {2500, 2343 }, {2747, 2553 }, {3000, 2763 }, {3270, 2958 }, {3508, 3152 }, {3749, 3282 }, {4017, 3416}},
        19,     
        SMP_DC,         CAL_PH_ABCN,        {3,  18}
    },

}},
{{///<630-400
    {/*Ch 0 RMS*/
        {},
        0,
        /**/
        SMP_CMP,        CAL_PH_ABCN,        {0,  0 }
    },
    {/*Ch 0 PEAK*/
        {},
        0,     
        SMP_CMP,        CAL_PH_ABCN,        {0,  0 }
    },
    {/*Ch 1 RMS*/
        {{0,    0    }, {200,  108  }, {400,  217  }, {750,  405  }, {1000, 538  }, {1500, 801  }, {2000, 1059 }, {2500, 1316 }, {3000, 1561 }, {3500, 1775 }, 
         {4000, 1966 }, {4500, 2117 }, {5000, 2248 }, {5500, 2343 }},
        14,
         /**/
        SMP_DC,         CAL_PH_ABCN,        {0,  3 }
    },
    {/*Ch 1 PEAK*/
        {{0,    0    }, {200,  152  }, {400,  307  }, {750,  577  }, {1000, 770  }, {1500, 1163 }, {2000, 1551 }, {2500, 1938 }, {3000, 2328 }, {3500, 2705 }, 
         {4000, 3075 }, {4500, 3421 }, {5000, 3736 }, {5500, 4000 }},
        14,     
        SMP_DC,         CAL_PH_ABCN,        {3,  18}
    }

}},
{{///<630-630
    {/*Ch 0 RMS*/
        {},
        0,
        /**/
        SMP_CMP,        CAL_PH_ABCN,        {0,  0 }
    },
    {/*Ch 0 PEAK*/
        {},
        0,     
        SMP_CMP,        CAL_PH_ABCN,        {0,  0 }
    },
    {/*Ch 1 RMS*/
        {{0,    0    }, {500,  162  }, {1000, 328  }, {1500, 492  }, {2000, 650  }, {2500, 811  }, {3000, 965  }, {4000, 1262 }, {5000, 1519 }, {6000, 1696 }, 
         {7000, 1846 }, {8000, 1940 }, {9000, 2051 }, {10000,2139}},
        14,
         /**/
        SMP_DC,         CAL_PH_ABCN,        {0,  3 }
    },
    {/*Ch 1 PEAK*/
        {{0,    0    }, {500,  231  }, {1000, 465  }, {1500, 701  }, {2000, 930  }, {2500, 1178 }, {3000, 1415 }, {4000, 1884 }, {5000, 2317 }, {6000, 2686 }, 
         {7000, 3032 }, {8000, 3326 }, {9000, 3612 }, {10000,3912}},
        14,     
        SMP_DC,         CAL_PH_ABCN,        {3,  18}
    }

}},
{{///<800-800
    {/*Ch 0 RMS*/
        {{0,    0    }, {500,  765  }, {1010, 1550 }, {1999, 3075 }, {4040, 5948 }},
        5,
         /**/
        SMP_SHIFT,      CAL_PH_ABC,         {0,  5 }
    },
    {/*Ch 0 PEAK*/
        {{0,    0    }, {500,  296  }, {1010, 566  }, {1999, 1112 }, {4040, 2047 }},
        5,     
        SMP_SHIFT,      CAL_PH_ABC,         {0,  5 }
    },
    {/*Ch 1 RMS*/
        {{0,    0    }, {500,  233  }, {1010, 469  }, {1999, 928  }, {4040, 1816 }, {6044, 2625 }, {11933,5165 }},
        7,
         /**/
        SMP_SHIFT,      CAL_PH_ABCN,        {5,  16}
    },
    {/*Ch 1 PEAK*/
        {{0,    0    }, {500,  108  }, {1010, 199  }, {1999, 350  }, {4040, 649  }, {6044, 932  }, {11933,1899 }},
        7,
         /**/
        SMP_SHIFT,      CAL_PH_ABCN,        {5,  16}
    }
}},
{{///<1600-1600
    {/*Ch 0 RMS*/
        {{0,    0    }, {503,  468  }, {1010, 943  }, {2005, 1867 }, {4009, 3693 }},
        5,
         /**/
        SMP_SHIFT,      CAL_PH_ABC,         {0,  3 }
    },
    {/*Ch 0 PEAK*/
        {{0,    0    }, {503,  168  }, {1010, 339  }, {2005, 658  }, {4009, 1315 }},
        5,     
        SMP_SHIFT,      CAL_PH_ABC,         {0,  3 }
    },
    {/*Ch 1 RMS*/
        {{0,    0    }, {503,  148  }, {1010, 297  }, {2005, 586  }, {4009, 1156 }, {8038, 2196 }, {11978,3363 }, {19888,5778 }},
        8,
         /**/
        SMP_SHIFT,      CAL_PH_ABCN,        {3,  16}
    },
    {/*Ch 1 PEAK*/
        {{0,    0    }, {503,  62   }, {1010, 112  }, {2005, 214  }, {4009, 419  }, {8038, 789  }, {11978,1203 }, {19888,2002 }},
        8,
         /**/
        SMP_SHIFT,      CAL_PH_ABCN,        {3,  16}
    }
}},
};

const struct custommeter::calculate::ChPhase custommeter::calculate::ChannelPhaseIndex[] = {
    //ChNum     Ch0-ADC-Channel         Ch1-ADC-Channel         NegtiveFlagCh
    {CAL_PH_A,  APP_ADC_IAL_CHANNEL,    APP_ADC_IAH_CHANNEL,    APP_ADC_GA_CHANNEL  },
    {CAL_PH_B,  APP_ADC_IBL_CHANNEL,    APP_ADC_IBH_CHANNEL,    APP_ADC_GB_CHANNEL  },
    {CAL_PH_C,  APP_ADC_ICL_CHANNEL,    APP_ADC_ICH_CHANNEL,    APP_ADC_GC_CHANNEL  },
    {CAL_PH_N,  APP_ADC_NO_CHANNEL,     APP_ADC_IN_CHANNEL,     APP_ADC_GN_CHANNEL  },
    {CAL_PH_G,  APP_ADC_NO_CHANNEL,     APP_ADC_IG_CHANNEL,     APP_ADC_NO_CHANNEL  }
};

const struct custommeter::calculate::ChAdc custommeter::calculate::ChAdcIndex[] = {
    {APP_ADC_IG_CHANNEL,    CH1,    CAL_PH_G },   
    {APP_ADC_ICL_CHANNEL,   CH0,    CAL_PH_C },
    {APP_ADC_IBL_CHANNEL,   CH0,    CAL_PH_B },
    {APP_ADC_IAL_CHANNEL,   CH0,    CAL_PH_A },
    {APP_ADC_ICH_CHANNEL,   CH1,    CAL_PH_C },
    {APP_ADC_IBH_CHANNEL,   CH1,    CAL_PH_B },
    {APP_ADC_IAH_CHANNEL,   CH1,    CAL_PH_A },
    {APP_ADC_IN_CHANNEL,    CH1,    CAL_PH_N },
		
    {APP_ADC_GC_CHANNEL,    CH0,    CAL_PH_C },
    {APP_ADC_GB_CHANNEL,    CH0,    CAL_PH_B },
    {APP_ADC_GA_CHANNEL,    CH0,    CAL_PH_A },
    {APP_ADC_GN_CHANNEL,    CH0,    CAL_PH_N },  

};
///> ADC 的一次采样回调函数，一周期64次
void custommeter::ConvertedCallBack(uint16_t *usResult, uint16_t usLen)
{
	uint16_t i, k, z;
	uint16_t *j = &EfectiveSampleBuff[mIndex];
	MeterChannelTPDF *channelptr;

	if(mLock)
	{
			return;
	}
	for(i = 0; i < usLen; i++)
	{
			j[i] = usResult[i];

	}
	mIndex += usLen;
	if(mIndex >= EfectiveSampleLen * ChannelCount)
	{
		mIndex = 0;
		if(!mFirstComplated)
		{
			mFirstComplated = true;
		}
	}
	calculate::FindPeakVal(EfectiveSampleBuff, mIndex, 0);
	calculate::FindPeakVal(EfectiveSampleBuff, mIndex, 1);
	calculate::FindPeakVal(EfectiveSampleBuff, mIndex, 2);
	calculate::FindPeakVal(EfectiveSampleBuff, mIndex, 3);
	calculate::FindPeakVal(EfectiveSampleBuff, mIndex, 4);
	calculate::FindPeakVal(EfectiveSampleBuff, mIndex, 5);
	calculate::FindPeakVal(EfectiveSampleBuff, mIndex, 6);
	calculate::FindPeakVal(EfectiveSampleBuff, mIndex, 7);
	InstantProtect();
	StartFilterFlag = 1;
	if(mFirstComplated)
	{
		mLock = true;
	}
}

void custommeter::CustomMeterThread(void *args)
{
    uint32_t i, j;
    int32_t k, sum, sum2;

    while(1)
    {
        Method::Sleep(1);

        if(mLock)
        {
            PrepareSampleData(EfectiveSampleBuff);
            for(i = 0; i < ChannelCount; i++)
            {
                MeterChannel[i].uiRealValue = CalculateCurrentVal(i);
            }
            TransCurrentVal();
            if(mIndex == 0)
            {
                CalGndCurrent();
            }
            DbgCalVal(0);
            if(ValueConvertedCallback)
            {
                ValueConvertedCallback();
            }
            mLock = false;
        }
    }
}
////> 基于DMA  ADC 的采样计量代码
bool custommeter::Start(adc::AdcInstanceTPDF tAdcInstance, uint32_t uiStackDeep, uint8_t ucThreadPrio)
{
    uint8_t i;
    uint32_t j = 0;

    if(!ChannelCount || !Frequnce || !SampleLen || !EfectiveSampleLen || !SampleBuff || !EfectiveSampleBuff)
    {
        return(false);
    }

    mFirstComplated = false;
    mLock = false;
    mIndex = 0;
    mAdc = new adc(tAdcInstance);

    for(i = 0; i < ChannelCount; i++)
    {
        MeterChannel[i].uiLimitCountUpper = 0;
        MeterChannel[i].bLimitEvent = false;
        j |= MeterChannel[i].tChannel;
    }

    mAdc->Channel = j;
    mAdc->Frequnce = Frequnce;
    mAdc->SampleLen = SampleLen;
    mAdc->ConvertCompleteCallBack = ConvertedCallBack; ///> 
    mAdc->Buff = SampleBuff;
    

    if(!mAdc->Start())
    {
        return(false);
    }

    threados *t1 = new threados(CustomMeterThread);
    t1->StackDeep = uiStackDeep;
    t1->Prio = ucThreadPrio;
    t1->Name = "cst mt";
    t1->Start();
    return(true);
}


void custommeter::InstantProtect(void)
{
    MeterChannelTPDF* channelptr;
    
    for(short i = 0; i < 8; i++)
    {
        channelptr = &MeterChannel[i];
        if(channelptr->bLimitUpperEnable)
        {
            if(InstantJdg(i))
            {
                if(channelptr->bLimitEvent)
                {
                    continue;
                }
                channelptr->bLimitEvent = true;

                if(ValueOutOfLimitCallback)
                {
                    ValueOutOfLimitCallback(i, 0, channelptr->iRecordUpper);
                }
            }

        }
        else
        {
            channelptr->uiLimitCountUpper = 0;
        }
    }
    
}


bool custommeter::calculate::InstantJdg(short channel)
{
    short CmpLvl;
    
    if(channel >= CUSTOMMETER_CHANNEL_MAX_SURPPORT)
    {
        return false;
    }
    
    if(ChAdcIndex[channel].Ch == CH0 && ChAdcIndex[channel].Phase == CAL_PH_N && IinlLvl != 0)
    {
        CmpLvl = IinlLvl;
    }
    else if(ChAdcIndex[channel].Ch == CH0 && (ChAdcIndex[channel].Phase | CAL_PH_ABC) != 0 && IilLvl != 0)
    {
        CmpLvl = IilLvl;
    }
    else if(ChAdcIndex[channel].Ch == CH1 && ChAdcIndex[channel].Phase == CAL_PH_N && IinhLvl != 0)
    {
        CmpLvl = IinhLvl;
    }
    else if(ChAdcIndex[channel].Ch == CH1 && (ChAdcIndex[channel].Phase | CAL_PH_ABC) != 0 && IihLvl != 0)
    {
        CmpLvl = IihLvl;
    }
    else
    {
        return false;
    }

    if(AdcPeakBuf[channel] > CmpLvl || SmpPeak[channel] > CmpLvl)
    {
        return true;
    }
    else
    {
        return false;
    }


}


void custommeter::calculate::RefreshCalculatePara(short IfrmSet, short InSet, short IiSetting, short IinSetting)
{
    short buf;
    unsigned char i;
    
    Ii = IiSetting;
    Iin = IinSetting;
    
    if(Ifrm != IfrmSet || In != InSet)
    {
        Ifrm = IfrmSet;
        In = InSet;
        buf = GetLinearIndex();
        pLinearTable = &LinearTableArray[buf];
        
        for(i = 0; i < sizeof(ChannelPhaseIndex)/sizeof(ChPhase); i++)
        {
            if(((1 << i) != ChannelPhaseIndex[i].Phase))
            {
                SEGGER_RTT_printf(0, "Channel mapping err\n");
                return;
            }
            
            if(pLinearTable->DataItem[INTERP_CH0_RMS].SmpType == SMP_SHIFT)
            {
                AdcMidLvlBuf[ChannelPhaseIndex[i].Ch0AdcSeqNum] = 2048;
            }
            else
            {
                AdcMidLvlBuf[ChannelPhaseIndex[i].Ch0AdcSeqNum] = 0;
            }
            
            if(pLinearTable->DataItem[INTERP_CH1_RMS].SmpType == SMP_SHIFT)
            {
                AdcMidLvlBuf[ChannelPhaseIndex[i].Ch1AdcSeqNum] = 2048;
            }
            else
            {
                AdcMidLvlBuf[ChannelPhaseIndex[i].Ch1AdcSeqNum] = 0;
            }
        }
    }
    
    if(pLinearTable->DataItem[INTERP_CH0_RMS].SmpType == SMP_DC || pLinearTable->DataItem[INTERP_CH0_RMS].SmpType == SMP_SHIFT)
    {
        IilLvl = CalAdcLinearInterp(INTERP_CH0_PEAK, Ii); 
        IinlLvl = CalAdcLinearInterp(INTERP_CH0_PEAK, Iin); 
    }
    else
    {
        IilLvl = 0;
        IinlLvl = 0;
    }
    
    if(pLinearTable->DataItem[INTERP_CH1_RMS].SmpType == SMP_DC || pLinearTable->DataItem[INTERP_CH1_RMS].SmpType == SMP_SHIFT)
    {
        IihLvl = CalAdcLinearInterp(INTERP_CH1_PEAK, Ii); 
        IinhLvl = CalAdcLinearInterp(INTERP_CH1_PEAK, Iin); 
    }
    else
    {
        IihLvl = 0;
        IinhLvl = 0;
    }
    
    return;
}

short custommeter::calculate::GetLinearIndex(void)
{
	short i;
	for(i = 0; i < sizeof(LinearIndexArray)/sizeof(LinearIndex); i++)
	{
		if(Ifrm == LinearIndexArray[i].IfrmType && In == LinearIndexArray[i].InType)
		{
			return LinearIndexArray[i].IndexNum;
		}
	}
	SEGGER_RTT_printf(0, "No prt cal array!!!!!!!\n");
	return 0;
}


short custommeter::calculate::CalCurrentLinearInterp(short Num, short InputVal)
{
    int val;
    unsigned char i;
    
    if(Num < 0 || Num >= 4)
    {
        return 0;
    }
    
    if(pLinearTable == NULL || (pLinearTable->DataItem[Num].SmpType != SMP_DC && pLinearTable->DataItem[Num].SmpType != SMP_SHIFT))
    {
        return 0;
    }
    
    val = 0;
    
    for(i = 1; i < pLinearTable->DataItem[Num].DataNum; i++)
    {
        if((InputVal < pLinearTable->DataItem[Num].DataTable[i].AdcData) || \
            (i == (pLinearTable->DataItem[Num].DataNum - 1)))
        {
            val  = (pLinearTable->DataItem[Num].DataTable[i].RealData - pLinearTable->DataItem[Num].DataTable[i-1].RealData);
            val *= ((int)InputVal - pLinearTable->DataItem[Num].DataTable[i-1].AdcData);
            val /= (pLinearTable->DataItem[Num].DataTable[i].AdcData - pLinearTable->DataItem[Num].DataTable[i-1].AdcData);
            val += (pLinearTable->DataItem[Num].DataTable[i-1].RealData);
            break;
        }
        else
        {
            continue;
        }
    }
    
    if(val > In * pLinearTable->DataItem[Num].Range[1])
    {
        val = In * pLinearTable->DataItem[Num].Range[1];
    }
    
    return val;
}


short custommeter::calculate::CalAdcLinearInterp(short Num, short InputVal)
{
    int val;
    unsigned char i;
    
    val = 0;
    
    if(Num < 0 || Num >= 4)
    {
        return 0;
    }
    
    if(pLinearTable == NULL || (pLinearTable->DataItem[Num].SmpType != SMP_DC && pLinearTable->DataItem[Num].SmpType != SMP_SHIFT))
    {
        return 0;
    }
    
    if(val > In * pLinearTable->DataItem[Num].Range[1])
    {
        return 0x7fff;
        //val = In * pLinearTable->DataItem[Num].Range[1];
    }
    for(i = 1; i < pLinearTable->DataItem[Num].DataNum; i++)
    {
       if(InputVal < pLinearTable->DataItem[Num].DataTable[i].RealData)
       {
           val  = (pLinearTable->DataItem[Num].DataTable[i].AdcData - pLinearTable->DataItem[Num].DataTable[i-1].AdcData);
           val *= ((int)InputVal - pLinearTable->DataItem[Num].DataTable[i-1].RealData);
           val /= (pLinearTable->DataItem[Num].DataTable[i].RealData - pLinearTable->DataItem[Num].DataTable[i-1].RealData);
           val += (pLinearTable->DataItem[Num].DataTable[i-1].AdcData);
           break;
       }
       else
       {
           continue;
       }
    }
    
    if(val > 4095)      ///<12bit ADC
    {
        val = 4095;
    }
    
   return val;

}

char custommeter::calculate::GetFilterArray(unsigned short const* pData, short DataPos)
{
    short i;
    
    if(pData == NULL || DataPos >= (ADC_CYC_DATA_NUM))
    {
        SEGGER_RTT_printf(0, "FilterArrayErr\n");
        return -1;
    }
    
    for(i = 0; i < ADC_CYCLE_NUM + CUR_FILTR_NUM; i++)
    {
        if(DataPos < 0)
        {
            DataPos += (SMP_LOOP_NUM * CUSTOMMETER_CHANNEL_MAX_SURPPORT);
        }
//        SEGGER_RTT_printf(0, "Cycle Data position %d\n", DataPos);
        FilterArrayBuf[ADC_CYCLE_NUM + CUR_FILTR_NUM - 1 - i] = pData[DataPos];
        
        DataPos -= CUSTOMMETER_CHANNEL_MAX_SURPPORT;
    }
    
    return 0;
}

void custommeter::calculate::Filter(char AdcChNum)
{
    short i;
    short buf;
    char NegFlag;
    
    for(i = 0; i < ADC_CYCLE_NUM; i++)
    {
        FilterArrayBuf[i] = FilterArrayBuf[i] + FilterArrayBuf[i+1] + FilterArrayBuf[i+2] + FilterArrayBuf[i+3];
        
        FilterArrayBuf[i] >>= 2;
        buf = FilterArrayBuf[i] - AdcMidLvlBuf[AdcChNum];
        
        if(pLinearTable->DataItem[ChAdcIndex[AdcChNum].Ch * 2].SmpType == SMP_SHIFT && buf < 0)
        {
            NegFlag = 1;
        }
        else
        {
            NegFlag = 0;
        }
        
        buf < 0 ? (buf = 0 - buf) : (buf = buf);
        if(AdcPeakBuf[AdcChNum] < buf)
        {
            AdcPeakBuf[AdcChNum] = buf;
            
            NegFlag ? NULL : (CurrPhPosBuf[AdcChNum] = AdcDataPos + i);
        }
    }
    
}

short custommeter::calculate::FindPeakVal(unsigned short* pData, short DataPos, short AdcChNum)
{
    short buf;
    char i;
    
    if(pData == NULL)
    {
        return -1;
    }

    AdcDataPos = ((DataPos >> ADC_CH_SHIFT) - ADC_CYCLE_NUM);
    AdcDataPos < 0 ? (AdcDataPos+=SMP_LOOP_NUM) : NULL;
    
    GetFilterArray(pData, (DataPos - CUSTOMMETER_CHANNEL_MAX_SURPPORT + AdcChNum));
    
    if(StartFilterFlag == 0)
    {
        for(i = 0; i < CUR_FILTR_NUM; i++)
        {
            FilterArrayBuf[i] = AdcMidLvlBuf[AdcChNum];
        }
    }
    
    Filter(AdcChNum);
    
    
    if(DataPos == 0)
    {
        SmpPeak[AdcChNum] = AdcPeakBuf[AdcChNum];
        AdcPeakBuf[AdcChNum] = 0;
        CurrPhPos[AdcChNum] = CurrPhPosBuf[AdcChNum];
        CurrPhPosBuf[AdcChNum] = 0;
        CurrPhPos[AdcChNum] < (SMP_LOOP_NUM>>1) ? NULL : CurrPhPos[AdcChNum] -= (SMP_LOOP_NUM>>1);
    }
    
    return 0;
}

bool custommeter::calculate::PrepareSampleData(unsigned short const* pSrc)
{
    if(pSrc == NULL)
    {
        return false;
    }
    
    MatSrc.numRows = SMP_LOOP_NUM;
    MatSrc.numCols = CUSTOMMETER_CHANNEL_MAX_SURPPORT;
    MatSrc.pData = (short*)pSrc;
    
    MatDst.numRows = CUSTOMMETER_CHANNEL_MAX_SURPPORT;
    MatDst.numCols = SMP_LOOP_NUM;
    MatDst.pData = (short*)SampleArrayBuf;
    Ch0Flag = 0;
    Ch1Flag = 0;
    
    arm_mat_trans_q15 (&MatSrc, &MatDst);
    
    return true;
}

short custommeter::calculate::CalRmsDC(short* pData, short AdcChNum)
{
    q63_t buf;
    
    arm_power_q15(pData, SMP_LOOP_NUM, &buf);
    
    buf /= SMP_LOOP_NUM;
    
    buf >>= 1;
    buf++;
    
    arm_sqrt_q31(buf, (q31_t*)&buf);
    
    buf >>= 15;

    return (short)buf;
}

short custommeter::calculate::CalRmsShift(short* pData, short AdcChNum)
{
    q63_t buf;
    arm_mean_q15 ((q15_t*)pData, SMP_LOOP_NUM, (q15_t*)&buf);
    arm_offset_q15((q15_t*)pData, 0-(q15_t)buf, (q15_t*)pData, SMP_LOOP_NUM);
    
    arm_power_q15(pData, SMP_LOOP_NUM, &buf);
    
    buf /= SMP_LOOP_NUM;
    
    buf >>= 1;
    buf++;
    
    arm_sqrt_q31(buf, (q31_t*)&buf);
    
    buf >>= 13;

    return (short)buf;
}

short custommeter::calculate::CalculateCurrentVal(short ChNum)
{
    char i;
    short buf;
    enum InterpItem ItpItmBuf;
    enum ChannelType Ch;
    
    Ch = NO_CH;
    Ch = ChAdcIndex[ChNum].Ch;
    if(Ch == NO_CH)
    {
        return 0;
    }
    if(((Ch == CH0 && pLinearTable->DataItem[INTERP_CH0_RMS].SmpType == SMP_DC)) ||\
        ((Ch == CH1 && pLinearTable->DataItem[INTERP_CH1_RMS].SmpType == SMP_DC)))
    {
        buf = CalRmsDC(&(SampleArrayBuf[ChNum*SMP_LOOP_NUM]), ChNum);
    }
    else if(((Ch == CH0 && pLinearTable->DataItem[INTERP_CH0_RMS].SmpType == SMP_SHIFT)) ||\
             ((Ch == CH1 && pLinearTable->DataItem[INTERP_CH1_RMS].SmpType == SMP_SHIFT)))
    {
        buf = CalRmsShift(&SampleArrayBuf[ChNum*SMP_LOOP_NUM], ChNum);
    }
    else
    {
        return 0;
    }
    
    SmpRms[ChNum] = buf;
    if(Ch == CH0)
    {
        ItpItmBuf = INTERP_CH0_RMS;
    }
    else if(Ch == CH1)
    {
        ItpItmBuf = INTERP_CH1_RMS;
    }
    else
    {
        return 0;
    }
    
    buf = CalCurrentLinearInterp(ItpItmBuf, buf);
    
    if(buf >= pLinearTable->DataItem[ItpItmBuf].Range[1] * In)
    {
        buf = CalCurrentLinearInterp(ItpItmBuf+1, SmpPeak[ChNum]);
    }
    
    CurrentVal[ChNum] = buf;
    
    return buf;
}

/** 
* @brief      	���Դ�ӡ
* @param[in]  	ChNum   	����ͨ����
* @retval     	None
*/
char TestCnt;
void custommeter::calculate::DbgCalVal(short ChNum)
{
#ifdef DBG_CURRENT_TEST

    unsigned short i;
    
//    if(ChNum == APP_ADC_IAL_CHANNEL)
    {
        TestCnt < 3 ? (TestCnt++) : (TestCnt=0);
    }
    if(TestCnt != 0)
    {
        return;
    }
//    if((ChNum < 4) || (ChNum == 6))
//    {
//        return;
//    }
    for(char i = 0; i < 6; i++)
    {
        if(SmpRms[i] > 50)
        {
    //        SEGGER_RTT_printf(0, "Ch %d Rms %d Peak %d Cur %d Pos %d\n", ChNum, SmpRms[ChNum],\
                SmpPeak[ChNum], CurrentVal[ChNum], CurrPhPos[ChNum]);
            SEGGER_RTT_printf(0, "Ch %d Rms %d Peak %d Cur %d\n", i, SmpRms[i],\
                SmpPeak[i], CurrentVal[i], AdcMidLvlBuf[i]);
        }
    }

//    for(i = 0; i < SMP_LOOP_NUM; i++)
//    {
//        SEGGER_RTT_printf(0, "Num:%d %d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\n", i,SampleArrayBuf[i],SampleArrayBuf[SMP_LOOP_NUM+i],SampleArrayBuf[SMP_LOOP_NUM*2+i],\
//        SampleArrayBuf[SMP_LOOP_NUM*3+i],SampleArrayBuf[SMP_LOOP_NUM*4+i],SampleArrayBuf[SMP_LOOP_NUM*5+i],SampleArrayBuf[SMP_LOOP_NUM*6+i],\
//        SampleArrayBuf[SMP_LOOP_NUM*7+i]);
//    }
#endif
}



char custommeter::calculate::TransCurrentVal(void)
{
    int buf;
    short i;
    bool reason[10];
    
    for(i = 0; i < 4; i++)
    {
//        reason[0] = (buf >= (pLinearTable->DataItem[INTERP_CH0_PEAK].Range[1] * In));
//        reason[1] = (buf <= (pLinearTable->DataItem[INTERP_CH1_PEAK].Range[1] * In));
//        reason[2] = ((pLinearTable->DataItem[INTERP_CH0_PEAK].Range[1]) >= (pLinearTable->DataItem[INTERP_CH1_RMS].Range[0]));
//        reason[3] = (pLinearTable->DataItem[INTERP_CH1_RMS].SmpType == SMP_DC || pLinearTable->DataItem[INTERP_CH1_RMS].SmpType == SMP_SHIFT);
//        reason[4] = ((pLinearTable->DataItem[INTERP_CH1_RMS].ApplyPh & (1 << i)) == (1 << i));
//        reason[5] = (ChannelPhaseIndex[i].Ch1AdcSeqNum != APP_ADC_NO_CHANNEL);
//        SEGGER_RTT_printf(0, "Ph %d r0 %d r1 %d r2 %d r3 %d r4 %d r5 %d\n", i, reason[0], reason[1], reason[2], reason[3], reason[4], reason[5]);
        
        if((pLinearTable->DataItem[INTERP_CH0_RMS].SmpType == SMP_DC || pLinearTable->DataItem[INTERP_CH0_RMS].SmpType == SMP_SHIFT)\
            && (ChannelPhaseIndex[i].Ch0AdcSeqNum != APP_ADC_NO_CHANNEL))
        {
            buf = CurrentVal[ChannelPhaseIndex[i].Ch0AdcSeqNum];
        }
        else
        {
            buf = pLinearTable->DataItem[INTERP_CH1_RMS].Range[0];
        }
        
        if((buf <= (pLinearTable->DataItem[INTERP_CH0_PEAK].Range[1] * In))
            &&((pLinearTable->DataItem[INTERP_CH0_RMS].ApplyPh & (1 << i)) == (1 << i)))
        {

            Ch0Flag |= (1<<i);
            Ch1Flag &= ~(1<<i);
        }
        if((buf >= (pLinearTable->DataItem[INTERP_CH0_PEAK].Range[1] * In)) \
             &&(buf <= (pLinearTable->DataItem[INTERP_CH1_PEAK].Range[1] * In)) \
             &&((pLinearTable->DataItem[INTERP_CH0_PEAK].Range[1]) >= (pLinearTable->DataItem[INTERP_CH1_RMS].Range[0]))\
             &&(pLinearTable->DataItem[INTERP_CH1_RMS].SmpType == SMP_DC || pLinearTable->DataItem[INTERP_CH1_RMS].SmpType == SMP_SHIFT) \
             &&((pLinearTable->DataItem[INTERP_CH1_RMS].ApplyPh & (1 << i)) == (1 << i))\
             &&(ChannelPhaseIndex[i].Ch1AdcSeqNum != APP_ADC_NO_CHANNEL))
        {
            buf = CurrentVal[ChannelPhaseIndex[i].Ch1AdcSeqNum];

            Ch1Flag |= (1<<i);
            Ch0Flag &= ~(1<<i);
        }
        CurrentBuf[i] = buf;
//        SEGGER_RTT_printf(0, "Ph %d Cur %d\n", i, buf);
    }
    return 0;
}

void custommeter::calculate::CalGndCurrent(void)
{
    int buf;
    short q15_buf;
    short RealBuf;
    short ImagBuf;
    char i;
    
    RealBuf = 0;
    ImagBuf = 0;
    for(i= 0; i < 4; i++)
    {
        if(pLinearTable->DataItem[INTERP_CH0_RMS].SmpType == SMP_CMP)
        {
            buf = CurrPhPos[ChannelPhaseIndex[i].Ch1AdcSeqNum];
            if(buf >= (SMP_LOOP_NUM >> 1))
            {
                CurrentBuf[4] = 0;
                return;
            }
            buf = SampleArrayBuf[ChannelPhaseIndex[i].ChNegtiveFlag * SMP_LOOP_NUM + buf];          ///<Find negetive flag of max val
//            SEGGER_RTT_printf(0, "Ph %d Ch %d Flag %d\n", i,ChannelPhaseIndex[i].ChNegtiveFlag,  buf);
            if(buf < SMP_ADC_MID_LVL)
            {

                buf = CurrPhPos[ChannelPhaseIndex[i].Ch1AdcSeqNum] + (SMP_LOOP_NUM >> 1);
            }
            else
            {
                buf = CurrPhPos[ChannelPhaseIndex[i].Ch1AdcSeqNum];
            }
        }
        else if(pLinearTable->DataItem[INTERP_CH0_RMS].SmpType == SMP_SHIFT)
        {
            if((Ch0Flag | (1<<i)) == (1<<i))
            {
                buf = CurrPhPos[ChannelPhaseIndex[i].Ch0AdcSeqNum];
            }
            else if((Ch1Flag | (1<<i)) == (1<<i))
            {
                buf = CurrPhPos[ChannelPhaseIndex[i].Ch1AdcSeqNum];
            }
        }
        else
        {
            CurrentBuf[4] = 0;
            return;
        }
        buf -= (SMP_LOOP_NUM >> 2);
        buf < 0 ? (buf += SMP_LOOP_NUM) : NULL;
//        SEGGER_RTT_printf(0, "Ph %d Ang %d ", i, buf);
        buf <<= 9;
        q15_buf = (short)buf;
//        SEGGER_RTT_printf(0, "Ph %d Ang mul %d \n", i, buf);
        q15_buf = buf;
        buf = arm_sin_q15((q15_t)q15_buf);
        buf *= CurrentBuf[i];
        buf >>= 15;
        RealBuf += buf;
//        SEGGER_RTT_printf(0, "Real %d ", i, buf);
        buf = arm_cos_q15((q15_t)q15_buf);
        buf *= CurrentBuf[i];
        buf >>= 15;
        ImagBuf += buf;
//        SEGGER_RTT_printf(0, "Imag %d\n", buf);
    }
//    SEGGER_RTT_printf(0, "Real %d Mag %d\n", RealBuf, ImagBuf);
    buf = (RealBuf*RealBuf) + (ImagBuf * ImagBuf);
    buf >>= 1;
    buf++;
    arm_sqrt_q31(buf, (q31_t*)&buf);
    buf >>= 15;
    CurrentBuf[4] = (short)buf;
//    SEGGER_RTT_printf(0, "Gnd Cur %d\n", buf);
    return;
}



